/*
 * IMU.h
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */

#ifndef IMU_H_
#define IMU_H_

#include "config.h"

/******************************************************************************/
/* IMU Configuration Defines                                                  */
/******************************************************************************/
/* Set the Low Pass Filter factor for ACC
   Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time
   Comment this if  you do not want filter at all.
   unit = n power of 2 */
/* this one is also used for ALT HOLD calculation, should not be changed*/
#ifndef ACC_LPF_FACTOR
    #define ACC_LPF_FACTOR      4 /* that means a LPF of 16 */
#endif

/* Set the Gyro Weight for Gyro/Acc complementary filter
   Increasing this value would reduce and delay Acc influence on the output of the filter*/
#ifndef GYR_CMPF_FACTOR
    #define GYR_CMPF_FACTOR     10 /*  that means a CMP_FACTOR of 1024 (2^10) */
#endif

/* Set the Gyro Weight for Gyro/Magnetometer complementary filter
   Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
#define GYR_CMPFM_FACTOR        8 /* that means a CMP_FACTOR of 256 (2^8) */

/********************************************************************************/
/* Macro Definition                                                             */
/********************************************************************************/
#define applyDeadband(value, deadband)  \
  if(abs(value) < deadband) {           \
    value = 0;                          \
  } else if(value > 0){                 \
    value -= deadband;                  \
  } else if(value < 0){                 \
    value += deadband;                  \
  }

/******************************************************************************/
/* Types Definition                                                           */
/******************************************************************************/
typedef struct  {
    int32_t X,Y,Z;
} t_int32_t_vector_def;

typedef struct  {
    uint16_t XL;
    int16_t  X;
    uint16_t YL;
    int16_t  Y;
    uint16_t ZL;
    int16_t  Z;
} t_int16_t_vector_def;

/* note: we use implicit first 16 MSB bits 32 -> 16 cast. ie V32.X>>16 = V16.X */
typedef union {
    int32_t A32[3];
    t_int32_t_vector_def V32;
    int16_t A16[6];
    t_int16_t_vector_def V16;
} t_int32_t_vector;

/******************************************************************************/
/* Exported Global Variables                                                  */
/******************************************************************************/
extern int16_t accZ;

/********************************************************************************/
/* Exported Functions Prototypes                                                */
/********************************************************************************/
extern void IMU_Init(void);
extern void IMU_Process(void);

/* IMU math functions */
extern void rotateV32( t_int32_t_vector *v,int16_t* delta);

#endif /* IMU_H_ */
